//
// Created by tatyx on 2025/8/25.
//
#include "Robstride_01.h"


FDCAN1_TxFrame_TypeDef hfdcan1_TxFrame_Robs = {
  .hcan   = &hfdcan1,
  .Header.IdType = FDCAN_EXTENDED_ID,
  .Header.TxFrameType = FDCAN_DATA_FRAME,
  .Header.DataLength = FDCAN_DLC_BYTES_8,
  .Header.ErrorStateIndicator = FDCAN_ESI_ACTIVE,
  .Header.BitRateSwitch = FDCAN_BRS_OFF,
  .Header.FDFormat = FDCAN_CLASSIC_CAN,
  .Header.TxEventFifoControl = FDCAN_NO_TX_EVENTS,
  .Header.MessageMarker = 0
};


void Robs_FDCAN_TxData_8byteS(FDCAN1_TxFrame_TypeDef *TxFrame, u8 *data, u8 masterID, u8 motorID,u8 mode)
{
  uint32_t id = (mode << 24) | (masterID << 8) | motorID;

  TxFrame->Header.Identifier = id;
  TxFrame->Data[0] = data[0];
  TxFrame->Data[1] = data[1];
  TxFrame->Data[2] = data[2];
  TxFrame->Data[3] = data[3];
  TxFrame->Data[4] = data[4];
  TxFrame->Data[5] = data[5];
  TxFrame->Data[6] = data[6];
  TxFrame->Data[7] = data[7];

  HAL_FDCAN_AddMessageToTxFifoQ(TxFrame->hcan, &TxFrame->Header,TxFrame->Data);
}


extern Robs_TypeDef Robs_motor01;
extern Robs_TypeDef Robs_motor02;
Robs_TypeDef Robs_motor03;
Robs_TypeDef Robs_motor04;

//使能电机，此处需要定义结构体或数组定义请求数据格式
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Enable_motor(Robs_TypeDef *motor)
{
  u8 data[8] = {0,0,0,0,0,0,0,0};
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,motor_enable);
}


//失能电机
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_DisEnable_motor(Robs_TypeDef *motor)
{
  u8 data[8] = {0,0,0,0,0,0,0,0};
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,motor_disable);
}


//设置当前位置为机械零位
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Set_Zero_position(Robs_TypeDef *motor)
{
  u8 data[8] = {0x01,0,0,0,0,0,0,0};
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,set_zero_position);
}


//设置电机ID，基本都在上位机中操作
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Set_Motor_ID(Robs_TypeDef *motor, u8 new_ID)
{
  u8 data[8] = {0,0,0,0,0,0,0,0};
  u16 id = ( new_ID << 8) | Robs_MasterID;
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,id, motor->motorID,set_zero_position);
}
//设置电机波特率，基本都在上位机中操作
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Set_Motor_Bute(Robs_TypeDef *motor, u8 new_bute)
{
  u8 data[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x00, 0x00};//单位K
  switch (new_bute)
  {
    case 1000:
    data[6] = 0x01;
    break;
    case 500:
    data[6] = 0x02;
    break;
    case 250:
    data[6] = 0x03;
    break;
    case 125:
    data[6] = 0x04;
    break;
  }
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,set_zero_position);
}


//读取电机参数，index寄存器地址详细看头文件
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Read_Parameters(Robs_TypeDef *motor,u16 index)
{
  u8 data[8] = {0,0,0,0,0,0,0,0};
  data[0] = index & 0xFF;
  data[1] = index >> 8;
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,read_parameters);
}

//写入电机参数，index寄存器地址详细看头文件
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Write_Parameters_float(Robs_TypeDef *motor,u16 index,float Value)
{
  u8 data[8] = {0,0,0,0,0,0,0,0};
  data[0] = index & 0xFF;
  data[1] = index >> 8;
  uint8_t* value_bytes = (uint8_t*)&Value;
  data[4] = value_bytes[0];  // 最低字节
  data[5] = value_bytes[1];
  data[6] = value_bytes[2];
  data[7] = value_bytes[3];
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,write_parameters);
}

void Robs_Write_Parameters_u8(Robs_TypeDef *motor,u16 index,u8 Value)
{
  u8 data[8] = {0,0,0,0,0,0,0,0};
  data[0] = index & 0xFF;
  data[1] = index >> 8;

  data[4] = Value;  // 最低字节
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,write_parameters);
}
//保存所有可储存变量
// 🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎🍏🍎
void Robs_Keep_Parameters(Robs_TypeDef *motor)
{
  u8 data[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08};
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,keep_parameters);
}


// 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀
uint16_t Map_To_Uint16(float value, float min_val, float max_val)
{
  if (value > max_val) value = max_val;
  if (value < min_val) value = min_val;

  return (uint16_t)((value - min_val) / (max_val - min_val) * 65535.0f);
}
//运动控制模式/MIT模式
// 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀
void Robs_Mit_mode(Robs_TypeDef *motor,float angle, float speed, float kp, float kd)
{
  u16 angle_u16 = Map_To_Uint16(angle, -12.57f, 12.57f);
  u16 speed_u16 = Map_To_Uint16(speed, -44.0f, 44.0f);
  u16 kp_u16 = Map_To_Uint16(kp, 0.0f, 500.0f);
  u16 kd_u16 = Map_To_Uint16(kd, 0.0f, 5.0f);
  u8 data[8] = {0};
  data[0] = angle_u16 & 0xFF;
  data[1] = angle_u16 >> 8;
  data[2] = speed_u16 & 0xFF;
  data[3] = speed_u16 >> 8;
  data[4] = kp_u16 & 0xFF;
  data[5] = kp_u16 >> 8;
  data[6] = kd_u16 & 0xFF;
  data[7] = kd_u16 >> 8;
  Robs_FDCAN_TxData_8byteS(&hfdcan1_TxFrame_Robs, data,Robs_MasterID, motor->motorID,motion_control);
}



//插补位置模式PP(参数position_PP_control)和 位置模式CSP(参数position_CSP_control)
// 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀
void Robs_Position_set(Robs_TypeDef *motor,u8 position_mode)
{
  Robs_Write_Parameters_u8(motor,run_mode,position_mode);
  HAL_Delay(10);
  Robs_Enable_motor(motor);
}
//插补位置模式PP
void Robs_Position_PP_control(Robs_TypeDef *motor, float speed, float acceleration, float position)
{
  Robs_Write_Parameters_float(motor,vel_max,speed);
  HAL_Delay(1);
  Robs_Write_Parameters_float(motor,acc_set,acceleration);
  HAL_Delay(1);
  Robs_Write_Parameters_float(motor,loc_ref,position);
}
//位置模式CSP
void Robs_Position_CSP_control(Robs_TypeDef *motor, float speed, float position)
{

  Robs_Write_Parameters_float(motor,limit_spd,speed);
  HAL_Delay(1);
  Robs_Write_Parameters_float(motor,loc_ref,position);
}


//速度模式
// 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀 🌈 💦 🌀
// void Robs_Speed_mode(0)

// 💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫
u8 Error_Report(u32 ID)
{
  u8 motor_id = (ID&0x0FF00)>>8;
  Robs_DisEnable_motor(&Robs_motor01);
  // Robs_DisEnable_motor(&motor02);
  // Robs_DisEnable_motor(&motor03);
  // Robs_DisEnable_motor(&motor04);
  ID &= 0x0FF0000;
  for (u8 i = 16; i < 21; i++)
  {
    if ((ID >> i) & 0x001)
    {
      return i;
      break;
    }
  }
}

// 💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫
float Value_calulation(uint8_t byte0, uint8_t byte1)
{
  uint16_t raw_value = (byte0 << 8) | byte1;
  float value = (float)raw_value / 65535.0f;
  return value;
}

//数据解析函数
// 💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫
void Robs_Read_0x02(Robs_TypeDef *motor, u8 *Data)
{
  motor->angle = Value_calulation(Data[0], Data[1])*25.14f-12.57f;
  motor->velocity = Value_calulation(Data[2], Data[3])*88.0f-44.0f;
  motor->Torque = Value_calulation(Data[4], Data[5])*34.0f-17.0f;
  u16 temp_raw= (Data[6]<<8 | Data[7]);
  motor->temprrature = temp_raw/10.0f;
}
// 💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫
void Robs_Read_0x11(Robs_TypeDef *motor, u8 *Data)
{
  motor->index = (Data[1]<<8) | Data[0];
  uint32_t raw_value = (Data[7] << 24) | (Data[6] << 16) |
                     (Data[5] << 8) | Data[4];
  motor->index_value = *((float*)&raw_value);
}


// 💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫・🌠・💫
void Robs_data_deal(FDCAN1_RxFrame_TypeDef *RxFrame)//0x   2  00 01   fd
{
  u8 Robs_motor_ID = (RxFrame->Header.Identifier&0x000FF00)>>8;
  if (Robs_motor_ID<=0x04)
  {
    u8 mode = RxFrame->Header.Identifier>>24;
    u8 fgh = (RxFrame->Header.Identifier&0x03F0000) >> 16;
    if (mode == 0x02)//
    {
      if (fgh != 0)//无故障
      {
        Error_Report(RxFrame->Header.Identifier);
      }
      switch (Robs_motor_ID)
      {
      case 0x01:
        Robs_Read_0x02(&Robs_motor01, RxFrame->Data);
        break;
      }
    }
    else if (mode == 0x11)
    {
      if (fgh == 0)//读取成功
      {
        switch (Robs_motor_ID)
        {
        case 0x01:
          Robs_Read_0x11(&Robs_motor01, RxFrame->Data);
          break;
        }
      }
  }
}
}